#include "wall_follow.h"
#include "debug_frmwrk.h"
#include "SERIALAPI.h"

#define     UARTx       LPC_UART0

void main(void)
{
    debug_frmwrk_init();
    init_wall_follow(23); // 23 to set the distance of the wollowing
    SERIAL_INIT();
    int32_t ticker = 1000000;
    UARTPuts(UARTx, "\n\rv1.0.0.20\n\rInit done, starting wall follow\n\r");
    
    while (ticker--)
    {
        // 0 is to do not change the last target distance set
        evaluate_step(1, 0);
        M1_FORWARD(get_m1());
        M2_FORWARD(get_m2());
    }
    UARTPuts(UARTx, "Task complete\n\r");
    return;
}
